/*
 * BarrierMovementEngine.cpp
 *
 *  Created on: 07-01-2011
 *      Author: Piotr
 */

#include "AI/fuzzyEngine/BarrierMovementEngine.hpp"
#include "AI/fuzzy/FuzzyUtils.hpp"
#include "AI/fuzzy/Fuzzifier3V.hpp"
#include "utils/MathUtils.hpp"
#include "utils/Randomizer.hpp"
#include "world/Individual.hpp"

void BarrierMovementEngine::calculate(float massOnTheLeft, float massInTheCenter,
		float massOnTheRight, float maxAngleChange, float maxValue, float closestElementDistance)
{
	Fuzzifier3V fuzzifiedLeft = Fuzzifier3V(massOnTheLeft, 0.0f, maxValue);
	Fuzzifier3V fuzzifiedCenter = Fuzzifier3V(massInTheCenter, 0.0f, maxValue);
	Fuzzifier3V fuzzifiedRight = Fuzzifier3V(massOnTheRight, 0.0f, maxValue);

	FuzzyValue l = fuzzifiedLeft.getSmall() + (-fuzzifiedCenter.getSmall())
			* (-fuzzifiedRight.getSmall());

	FuzzyValue c = (-fuzzifiedLeft.getSmall()) * (-fuzzifiedRight.getSmall())
			+ fuzzifiedCenter.getSmall();

	FuzzyValue r = (-fuzzifiedLeft.getSmall()) * (-fuzzifiedCenter.getSmall())
			+ fuzzifiedRight.getSmall();

	FuzzyValue rand1 = fuzzifiedLeft.getBig() * fuzzifiedCenter.getBig() * fuzzifiedRight.getBig();

	FuzzyValue rand2 = fuzzifiedLeft.getMedium() * fuzzifiedCenter.getMedium()
			* fuzzifiedRight.getMedium();

	FuzzyValue rand3 = fuzzifiedLeft.getSmall() * fuzzifiedCenter.getSmall()
			* fuzzifiedRight.getSmall();

	float toLeft = l.getFloatValue();
	float inCenter = c.getFloatValue();
	float toRight = r.getFloatValue();
	float toRand = MathUtils::max(3, rand1.getFloatValue(), rand2.getFloatValue(),
			rand2.getFloatValue());
	float max = MathUtils::max(4, toLeft, inCenter, toRight, toRand);

	if(MathUtils::areEqual(3, toLeft, inCenter, toRight) || closestElementDistance <= 2
			* Individual::SIZE)
	{
		if(Randomizer::nextBool())
		{
			angleChange = -maxAngleChange;
		}
		else
		{
			angleChange = maxAngleChange;
		}
		speedFactor = 0.0f;
	}
	else if(toLeft == max)
	{
		angleChange = -maxAngleChange;
		speedFactor = toLeft;
	}
	else if(toRight == max)
	{
		angleChange = maxAngleChange;
		speedFactor = toRight;
	}
	else if(inCenter == max)
	{
		max = MathUtils::max(2, toLeft, toRight);
		if(max == toLeft)
		{
			angleChange = -(inCenter / 2) * maxAngleChange;
			speedFactor = inCenter;
		}
		else
		{
			angleChange = (inCenter / 2) * maxAngleChange;
			speedFactor = inCenter;
		}
	}
	else
	{
		if(Randomizer::nextBool())
		{
			angleChange = -maxAngleChange;
		}
		else
		{
			angleChange = maxAngleChange;
		}
		speedFactor = 0.0f;
	}
}

float BarrierMovementEngine::getAngleChange() const
{
	return angleChange;
}

float BarrierMovementEngine::getSpeedFactor() const
{
	return speedFactor;
}
